#include "node_base.h"
#include "common_tool/angle_helper.h"
#include "perpend_point2d_helper.h"
#include <cmath>
#include <limits>
#include <climits>
#include "common_tool/geometry.hpp"
#include <memory>
#include <chrono>
#include "node_config_path.h"
#include "node_log_data_path.h"

namespace auto_ros
{
namespace control
{
NodeBase::NodeBase(double loop_rate, std::string node_yaml_file_name) : loop_rate_(loop_rate), rate_(loop_rate)

{

	node_yaml_ = YAML::LoadFile(node_config_path + node_yaml_file_name);
	std::fstream fout(node_config_path + node_yaml_file_name + ".cor", std::ios::out);
	fout << node_yaml_;
	fout.close();
	log_file_name_ = node_log_data_path + node_yaml_["test_speed"].as<std::string>() + "_" +
					 node_yaml_["output_csv"].as<std::string>();
	log_writer_ptr_ = std::make_shared<io::CSVWriter>(log_file_name_);
	localizition_dog_.setAllParameter(8, 1, 0, 3, 8);
	chassis_dog_.setAllParameter(8, 1, 0, 3, 8);
	planning_dog_.setAllParameter(8, 1, 0, 17, 25);
	auto_msg_dog_.setAllParameter(8, 1, 0, 17, 25);
}
void NodeBase::CBLocalizition(const auto_msgs::localizition &msg)
{
	localizition_msg_ = msg;
	localizition_dog_.giveFood();
	//std::cout<<"************************"<<localizition_msg_.ax<<std::endl;
}
void NodeBase::CBChassis(const auto_msgs::chassis &msg)
{
	chassis_msg_ = msg;
	chassis_dog_.giveFood();
}
void NodeBase::CBPlanning(const auto_msgs::planning &msg)
{
	//ROS_INFO("[NodeBase::CBPlanning] has received msg,msg size is:%d", msg.path.size());
	planning_msg_ = msg;
	planning_dog_.giveFood();
}
void NodeBase::CBAuto(const auto_msgs::auto_ctr &msg)
{
	auto_ctr_msg_ = msg;
	auto_msg_dog_.giveFood();
}
bool NodeBase::SearchRefPoint(const auto_msgs::planning planning_msg) //search point relative to center of grativity
{
	auto_msgs::localizition localizition_msg = localizition_msg_;
	if (planning_msg.path.size() <= 0)
	{
		ROS_ERROR("[NodeLateral::SearchRefPoint] failed due to path size is zero");
		return false;
	}
	else
	{
		double heading_arc = localizition_msg.heading * 3.14 / 180;
		double x_cg = localizition_msg.x; //+ lr * std::cos(heading_arc);
		double y_cg = localizition_msg.y; //+ lr * std::sin(heading_arc);
		double last_closet_distance = std::numeric_limits<double>::max();
		double last_2nd_closet_distance = std::numeric_limits<double>::max();
		int closet_point_num = std::numeric_limits<int>::max();
		int second_closet_point_num = std::numeric_limits<int>::max();
		for (int i = 0; i < planning_msg.path.size(); i++) //search closet point to center of grativity
		{

			double path_x_cg_global = planning_msg.path[i].x - x_cg; //car cg point to path point vector in gloabal frame
			double path_y_cg_global = planning_msg.path[i].y - y_cg;
			//car cg point to path point vector in car frame
			double path_x_cg_car = path_x_cg_global * std::cos(heading_arc) +
								   path_y_cg_global * std::sin(heading_arc);
			double path_y_cg_car = -1.0 * path_x_cg_global * std::sin(heading_arc) +
								   path_y_cg_global * std::cos(heading_arc);
			double distance = std::pow(path_x_cg_car * path_x_cg_car + path_y_cg_car * path_y_cg_car, 0.5);
			if (distance < last_2nd_closet_distance)
			{
				if (distance <= last_closet_distance)
				{
					second_closet_point_num = closet_point_num;
					last_2nd_closet_distance = last_closet_distance;
					closet_point_num = i;
					last_closet_distance = distance;
				}
				else
				{
					second_closet_point_num = i;
					last_2nd_closet_distance = distance;
				}
			}
		}

		std::cout << "closet_point_num is " << closet_point_num << std::endl;
		std::cout << "second_closet_point_num is " << second_closet_point_num << std::endl;
		if (closet_point_num >= planning_msg.path.size() || second_closet_point_num >= planning_msg.path.size())
		{
			ROS_ERROR("[Node2d2s::SearchRefPoint] failed due to closet_point_num>path.size()");
			std::cout << "path size is:" << planning_msg.path.size() << std::endl;
			return false;
		}
		if (std::abs(closet_point_num - second_closet_point_num) > 5)
		{
			ROS_ERROR("[NodeLateral::SearchRefPoint] failed due to std::abs(closet_point_num-second_closet_point_num)>2 ");
			return false;
		}
		std::cout << "closet point is :" << planning_msg.path[closet_point_num] << std::endl;
		ref_point_closet_ = planning_msg.path[closet_point_num];
		ref_point_second_closet_ = planning_msg.path[second_closet_point_num];
		auto_msgs::planning_point vehicle_cg_point;
		vehicle_cg_point.x = x_cg;
		vehicle_cg_point.y = y_cg;
		if (!perpend_point2d<auto_msgs::planning_point>(vehicle_cg_point, ref_point_closet_,
														ref_point_second_closet_, ref_lambda_))
		{
			std::cout << "\033[perpend_point2d] in [NodeLateral::SearchRefPoint]failed  \033[0m"
					  << std::endl;
			return false;
		}

		else
		{
			if (std::abs(ref_lambda_) > 2)
			{
				ROS_ERROR("std::abs(ref_lambda_) > 2");
				return false;
			}
			ref_point_interp_.theta = auto_ros::common_tool::angle_mins_180to180(ref_lambda_ * auto_ros::common_tool::angle_mins_180to180(ref_point_closet_.theta - ref_point_second_closet_.theta) +
																				 ref_point_second_closet_.theta);
			//globol frame
			ref_point_interp_.x = ref_lambda_ * ref_point_closet_.x + (1 - ref_lambda_) * ref_point_second_closet_.x;
			ref_point_interp_.y = ref_lambda_ * ref_point_closet_.y + (1 - ref_lambda_) * ref_point_second_closet_.y;
			std::cout << "*******" << std::pow(std::pow(ref_point_interp_.x - x_cg, 2) + std::pow(ref_point_interp_.y - y_cg, 2.0), 0.5);

			ref_point_interp_.x = ref_point_interp_.x - x_cg;
			ref_point_interp_.y = ref_point_interp_.y - y_cg;
			lateral_error_ = -ref_point_interp_.x * std::sin(ref_point_interp_.theta * M_PI / 180) +
							 ref_point_interp_.y * std::cos(ref_point_interp_.theta * M_PI / 180);
			//transform into car frame
			double temp_x = ref_point_interp_.x;
			ref_point_interp_.x = ref_point_interp_.x * std::cos(heading_arc) +
								  ref_point_interp_.y * std::sin(heading_arc);

			ref_point_interp_.y = -1.0 * temp_x * std::sin(heading_arc) +
								  ref_point_interp_.y * std::cos(heading_arc);

			ref_point_interp_.kappa = ref_lambda_ * ref_point_closet_.kappa +
									  (1 - ref_lambda_) * ref_point_second_closet_.kappa;
			ref_point_interp_.v = ref_lambda_ * ref_point_closet_.v +
								  (1 - ref_lambda_) * ref_point_second_closet_.v;
			ref_point_interp_.s = ref_lambda_ * ref_point_closet_.s +
								  (1 - ref_lambda_) * ref_point_second_closet_.s;
			ref_point_interp_.a = ref_lambda_ * ref_point_closet_.a +
								  (1 - ref_lambda_) * ref_point_second_closet_.a;
		}
	}

	return true;
}
bool NodeBase::CheckDog()
{
	bool localizition_flag = false;
	bool chassis_flag = false;
	bool planning_flag = false;
	if (localizition_dog_.isDied())
	{
		localizition_flag = true;
		ROS_ERROR("NodeBase::CheckDog():localizition_dog time out666666");
	}
	if (chassis_dog_.isDied())
	{
		chassis_flag = true;
		ROS_ERROR("NodeBase::CheckDog():chassis_dog time out");
	}
	if (planning_dog_.isDied())
	{
		planning_flag = true;
		ROS_ERROR("NodeBase::CheckDog():planning_dog time out");
	}
	if (localizition_flag || chassis_flag || planning_flag)
	{
		return false;
	}
	return true;
}
void NodeBase::NoFood()
{
	localizition_dog_.noFood();
	chassis_dog_.noFood();
	planning_dog_.noFood();
}

} // namespace control
} // namespace auto_ros